#include<iostream>
#include "coordinate_ransformation.h"

Coordinate_ransformation::Coordinate_ransformation()
{
    cameraMatrix = cv::Mat(3, 3, CV_64F);

    distCoeffs = cv::Mat(1, 5, CV_64F);

    zConst = 0;

    rotationMatrix = cv::Mat (3, 3, CV_64F);

    tvec = cv::Mat (3, 1, CV_64F);

    Y_offset = 0;
    
    X_offset = 0;
}

// bool Coordinate_ransformation::LoadIntrinsic(const std::string &file_path) 
// {
//     try 
//     {
//         std::vector<double> tempMatrix;
//         std::vector<double> tempdistCoeff;
//         YAML::Node config = YAML::LoadFile(file_path);
//         tempMatrix = config["camera_matrix"]["data"].as<std::vector<double>>();
//         tempdistCoeff = config["distortion_coefficients"]["data"].as<std::vector<double>>();
//         for (int i = 0; i < 3; i++) 
//         {
//             for (int j = 0; j < 3; j++) 
//             {
//                 cameraMatrix.at<double>(i, j) = tempMatrix[3 * i + j];
//             }
//         }
//         for (int i = 0; i < 5; i++) {
//             distCoeffs.at<double>(i) = tempdistCoeff[i];
//         }
//         //std::cout << "camera_matrix:" << std::endl << cameraMatrix << std::endl << "distCoeffs:" << std::endl << distCoeffs << std::endl; 
//     } 
//     catch (const YAML::Exception &e) 
//     {
//         std::cout << "load Intrinsic: " << file_path << " failed! error: " << e.what() << std::endl;
//         return false;
//     }
//     return true;
// }

// bool Coordinate_ransformation::LoadExternal(const std::string &file_path) 
// {
//     try 
//     {
//         YAML::Node config = YAML::LoadFile(file_path);
//         if (config["rotation"] && config["translation"]) 
//         {
//             rotationMatrix.at<double>(0, 0) = config["rotation"]["0,0"].as<double>();
//             rotationMatrix.at<double>(0, 1) = config["rotation"]["0,1"].as<double>();
//             rotationMatrix.at<double>(0, 2) = config["rotation"]["0,2"].as<double>();
//             rotationMatrix.at<double>(1, 0) = config["rotation"]["1,0"].as<double>();
//             rotationMatrix.at<double>(1, 1) = config["rotation"]["1,1"].as<double>();
//             rotationMatrix.at<double>(1, 2) = config["rotation"]["1,2"].as<double>();
//             rotationMatrix.at<double>(2, 0) = config["rotation"]["2,0"].as<double>();
//             rotationMatrix.at<double>(2, 1) = config["rotation"]["2,1"].as<double>();
//             rotationMatrix.at<double>(2, 2) = config["rotation"]["2,2"].as<double>();

//             tvec.at<double>(0) = config["translation"]["x"].as<double>();
//             tvec.at<double>(1) = config["translation"]["y"].as<double>();
//             tvec.at<double>(2) = config["translation"]["z"].as<double>();
//         }
//        // std::cout << "rotation:" << std::endl << rotationMatrix << std::endl << "translation:" << std::endl << tvec << std::endl; 
//     }
//     catch (const YAML::Exception &e) 
//     {
//         std::cout << "load Extrinsics: " << file_path << " failed! error: " << e.what() << std::endl;
//         return false;
//     }
//     return true;
// }

cv::Point3f Coordinate_ransformation::getWorldPoints(cv::Point2f inPoints)
{
    //计算参数
    double s;

    //获取图像坐标
    cv::Mat imagePoint = cv::Mat::ones(3, 1, CV_64F); //u,v,1
	imagePoint.at<double>(0, 0) = inPoints.x;
	imagePoint.at<double>(1, 0) = inPoints.y;
 
	//计算比例参数S
	cv::Mat tempMat, tempMat2;
	tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
	tempMat2 = rotationMatrix.inv() * tvec;
	s = zConst + tempMat2.at<double>(2, 0);
	s /= tempMat.at<double>(2, 0);
 
    //计算世界坐标
	cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
	cv::Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
	return worldPoint;
}

cv::Point3d Coordinate_ransformation::getfloorPoints(cv::Point inPoints)
{
    //计算参数
    double s;

    //获取图像坐标
    cv::Mat imagePoint = cv::Mat::ones(3, 1, CV_64F); //u,v,1
	imagePoint.at<double>(0, 0) = inPoints.x;
	imagePoint.at<double>(1, 0) = inPoints.y;
 
	//计算比例参数S
	cv::Mat tempMat, tempMat2;
	tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
	tempMat2 = rotationMatrix.inv() * tvec;
	s = zConst + tempMat2.at<double>(2, 0);
	s /= tempMat.at<double>(2, 0);
 
    //计算世界坐标
	cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
	cv::Point3d worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), 0);
	// cv::Point worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0));
	return worldPoint;
}

cv::Point2f Coordinate_ransformation::getUVPoints(cv::Point3d worldPoint)  
{  
    //    [fx s x0]                         [Xc]        [Xw]        [u]   1     [Xc]  
    //K = |0 fy y0|       TEMP = [R T]      |Yc| = TEMP*|Yw|        | | = —*K *|Yc|  
    //    [ 0 0 1 ]                         [Zc]        |Zw|        [v]   Zc    [Zc]  
    //                                                  [1 ]  
    cv::Point3f c;  
    c.x = rotationMatrix.at<double>(0, 0)*worldPoint.x + rotationMatrix.at<double>(0, 1)*worldPoint.y + rotationMatrix.at<double>(0, 2)*worldPoint.z + tvec.at<double>(0, 0)*1;  
    c.y = rotationMatrix.at<double>(1, 0)*worldPoint.x + rotationMatrix.at<double>(1, 1)*worldPoint.y + rotationMatrix.at<double>(1, 2)*worldPoint.z + tvec.at<double>(0, 1)*1;  
    c.z = rotationMatrix.at<double>(2, 0)*worldPoint.x + rotationMatrix.at<double>(2, 1)*worldPoint.y + rotationMatrix.at<double>(2, 2)*worldPoint.z + tvec.at<double>(0, 2)*1;  
  
    cv::Point2f uv;  
    uv.x = (cameraMatrix.at<double>(0, 0)*c.x + cameraMatrix.at<double>(0, 1)*c.y + cameraMatrix.at<double>(0, 2)*c.z)/c.z;  
    uv.y = (cameraMatrix.at<double>(1, 0)*c.x + cameraMatrix.at<double>(1, 1)*c.y + cameraMatrix.at<double>(1, 2)*c.z)/c.z;  
  
    return uv;  
}  

void Coordinate_ransformation::Calculate_target_distance(cv::Point2f inPoints_left , cv::Point2f inPoints_right, double *longitudinal,double *lateral,double *veh_width)
{
    cv::Point3f world_point_left = getWorldPoints(inPoints_left);
    cv::Point3f world_point_right = getWorldPoints(inPoints_right);

    // std::cout << "world_point_left: " << world_point_left  << std::endl;
    // std::cout << "world_point_right: " << world_point_right  << std::endl;

    *longitudinal = MIN(world_point_left.y, world_point_right.y) + Y_offset;
    *lateral = (world_point_right.x + world_point_left.x)/2 + X_offset;
    *veh_width = world_point_right.x - world_point_left.x;
}
